// --------------------------------------------------------------------------------------------------------------------
// <copyright file="Ev4FloorFilterTests.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Test.Robotics.Runtime
{
    using System;
    using System.Drawing;
    using System.IO;
    using System.Reflection;
    using System.Runtime.Serialization;
    using System.Threading;
    using Microsoft.Robotics.Numerics;
    using Microsoft.Robotics.Runtime;
    using Microsoft.Robotics.Vision.Cameras;
    using Microsoft.Robotics.Vision.Runtime.Cameras;
    using Microsoft.VisualStudio.TestTools.UnitTesting;
    using Test.Robotics.Runtime;

    /// <summary>
    /// Validates that message synchronization works as expected.
    /// </summary>
    [TestClass]
    public class Ev4FloorFilterTests
    {
        /// <summary>
        /// Tests if Filter Reports Correct State When Not Initialized
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void FilterReportsCorrectStateWhenNotInitialized()
        {
            Ev4FloorFilter filter = new Ev4FloorFilter(null, 0, 0, 0, 0);

            Assert.IsFalse(filter.IsInitialized()); 
        }

        /// <summary>
        /// Tests if Filter Reports Correct State When Initialized
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void FilterReportsCorrectStateWhenInitialized()
        {
            IFlatSurfaceLearnDataPruner pruner = new Ev4FloorDataPruner(0.6, 0.4, 1, 1, 1);

            Ev4FloorFilter filter = new Ev4FloorFilter(pruner, 0, 0, 0, 0);

            filter.SetCalibrationData(FloorFilterTestHelper.GetPerfectCalibrationData());

            Assert.IsTrue(filter.IsInitialized());
        }

        /// <summary>
        /// Tests if Correct Constants Are Generated When Seeing Flat Floor
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void CorrectConstantsAreGeneratedWhenSeeingFlatFloor()
        {
            double epsilon = 0.01;

            Ev4FloorFilter filter = new Ev4FloorFilter(null, 0, 0, 0, 0);

            // Exact A and B for this data are: 200, and 320000 respectively. Collected from a real data sample off Ev4
            // running with resolution 640x480
            double[] profile = new double[] { 1167.883212, 1163.636364, 1159.42029, 1155.234657, 1151.079137, 1146.953405 };

            int startIndex = 480 - profile.Length;

            FastTuple<double, double, double> result;

            filter.FindFittingConstantsForStrip(
                profile, 
                profile.Length, 
                startIndex, 
                0, 
                100, 
                0.001, 
                10,
                out result);

            Assert.IsTrue(Math.Abs(result.Item1 - 200) < epsilon); // A
            Assert.IsTrue(Math.Abs(result.Item2 - 320000) < 1); // B is a large value, and may vary a fair bit in absolute terms
            Assert.IsTrue(result.Item3 < epsilon); // Variance
        }

        /// <summary>
        /// Tests if Variance Is Great When Floor Is Not Very Flat
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void VarianceIsGreatWhenFloorIsNotVeryFlat()
        {
            double largeError = 10;

            Ev4FloorFilter filter = new Ev4FloorFilter(null, 0, 0, 0, 0);

            // Exact A and B for this data are: 200, and 320000 respectively. Collected from a real data sample off Ev4
            // running with resolution 640x480, but first datapoint is changed to make floor look less flat 
            double[] profile = new double[] { 1155, 1163.636364, 1159.42029, 1155.234657, 1151.079137, 1146.953405 };

            int startIndex = 480 - profile.Length;

            FastTuple<double, double, double> result;

            filter.FindFittingConstantsForStrip(
                profile,
                profile.Length,
                startIndex,
                0,
                100,
                0.001,
                10,
                out result);

            Assert.IsTrue(result.Item3 > largeError);
        }

        /// <summary>
        /// Tests if Floor Is Learned When Frame Contains Floor
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void FloorIsLearnedWhenFrameContainsPerfectFloor()
        {
            int width = 80;
            int height = 60;

            short[] depthFrame = FloorFilterTestHelper.SynthesizeDepthFrameWithFloor(width, height, 45, 30, 300000).Item1;

            IFlatSurfaceLearnDataPruner pruner = new Ev4FloorDataPruner(0.6, 0.4, 1, 15, 5);
            Ev4FloorFilter filter = new Ev4FloorFilter(pruner, 0, 0, 0, 0);

            filter.SetCalibrationData(FloorFilterTestHelper.GetPerfectCalibrationData());

            bool floorLeadned = filter.TryLearnFloor(depthFrame, width, height);

            Assert.IsTrue(floorLeadned);
        }

        /// <summary>
        /// Tests if Floor Is Learned When Frame Contains noisy Floor
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void FloorIsLearnedWhenFrameContainsPartialFloor()
        {
            int width = 80;
            int height = 60;

            short[] depthFrame = FloorFilterTestHelper.SynthesizeDepthFrameWithFloor(width, height, 45, 30, 300000).Item1;

            // Mess up first strip of depth frame. We have 4 strips, so introducing noise into part of the first strip should
            // not throw off the learner
            int index = 0;
            for (int i = 0; i < height; ++i)
            {
                for (int j = 0; j < width; ++j)
                {
                    if (j < width / 5)
                    {
                        depthFrame[index] = 2000;
                    }

                    ++index;
                }
            }

            IFlatSurfaceLearnDataPruner pruner = new Ev4FloorDataPruner(0.6, 0.4, 1, 15, 5);
            Ev4FloorFilter filter = new Ev4FloorFilter(pruner, 0, 0, 0, 0);

            filter.SetCalibrationData(FloorFilterTestHelper.GetPerfectCalibrationData());

            bool floorLeadned = filter.TryLearnFloor(depthFrame, width, height);

            Assert.IsTrue(floorLeadned);
        }

        /// <summary>
        /// Tests if Floor Is Not Learned When Frame Contains too much noise
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void FloorIsNotLearnedWhenFrameContainsTooMuchNoise()
        {
            int width = 80;
            int height = 60;

            short[] depthFrame = FloorFilterTestHelper.SynthesizeDepthFrameWithFloor(width, height, 45, 30, 300000).Item1;

            // Mess up first half + 10% of depth frame. This will leave only 1 out 4 strips healthy, which should fail, since our
            // threshold is > 25% of strips must be healthy
            int index = 0;
            for (int i = 0; i < height; ++i)
            {
                for (int j = 0; j < width; ++j)
                {
                    if (j < (width / 2 + width / 10))
                    {
                        depthFrame[index] = 2000;
                    }

                    ++index;
                }
            }

            IFlatSurfaceLearnDataPruner pruner = new Ev4FloorDataPruner(0.6, 0.4, 1, 1, 1);
            Ev4FloorFilter filter = new Ev4FloorFilter(pruner, 0, 0, 0, 0);

            filter.SetCalibrationData(FloorFilterTestHelper.GetPerfectCalibrationData());

            bool floorLeadned = filter.TryLearnFloor(depthFrame, width, height);

            Assert.IsFalse(floorLeadned);
        }

        /// <summary>
        /// Tests if Floor Is not Learned When Frame Contains no Floor
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void FloorIsNotLearnedWhenFrameContainsNoFloor()
        {
            int width = 80;
            int height = 60;

            // starting floor at 61 means no floor will be generated
            short[] depthFrame = FloorFilterTestHelper.SynthesizeDepthFrameWithFloor(width, height, height + 1, 30, 300000).Item1;

            IFlatSurfaceLearnDataPruner pruner = new Ev4FloorDataPruner(0.6, 0.4, 1, 1, 1);
            Ev4FloorFilter filter = new Ev4FloorFilter(pruner, 0, 0, 0, 0);

            filter.SetCalibrationData(FloorFilterTestHelper.GetPerfectCalibrationData());

            bool floorLeadned = filter.TryLearnFloor(depthFrame, width, height);

            Assert.IsFalse(floorLeadned);
        }

        /// <summary>
        /// Tests if CorrectNumberOfFloorPixelsMarkedWhenSceneContainsFloor
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void CorrectNumberOfFloorPixelsMarkedWhenSceneContainsFloor()
        {
            int width = 80;
            int height = 60;

            MockFloorFilterPruner pruner = new MockFloorFilterPruner();

            pruner.DataPointsA.Add(new FastTuple<double, double>(2, height / 2));
            pruner.DataPointsA.Add(new FastTuple<double, double>(20, height / 2 - 1));

            pruner.DataPointsB.Add(new FastTuple<double, double>(2, 35000));
            pruner.DataPointsB.Add(new FastTuple<double, double>(20, 36000));
            
            pruner.DataPointsD.Add(new FastTuple<double, double>(2, 950));
            pruner.DataPointsD.Add(new FastTuple<double, double>(20, 954));

            Ev4FloorFilter filter = new Ev4FloorFilter(pruner, 0, 0, 0, 0);

            filter.SetCalibrationData(FloorFilterTestHelper.GetRealisticCalibrationData(height, width));

            var result = FloorFilterTestHelper.SynthesizeDepthFrameWithFloor(width, height, height / 2, height / 3, 40000);

            short[] depthFrame = result.Item1;
            int realFloorPixelCount = result.Item2;

            filter.FilterPlanesOnFrame(depthFrame, width, height);

            // Go pixel by pixel and verify we marked floor fully
            int markedFloorPixelCount = FloorFilterTestHelper.CountMarkedFloorPixels(depthFrame, width, height, height / 2, height / 3, 40000);

            Assert.AreEqual(realFloorPixelCount, markedFloorPixelCount);
        }
    }
}
